import ustruct
import usr.modbus as modbus

class ModbusSlave:
    def __init__(self) -> None:
        self.__buf_size = 255
        self.__buf = bytearray(self.__buf_size)
        self.__start_address = 0
    
    def __del__(self):
        self.__buf.clear()
        del self.__buf

    def parse(self, req):
        status = 0
        resp = None
        resfunc = 0
        error = 0
        rlen = len(req)
        if rlen < 5:
            return (status, resp)
        sub = req[0]
        func = req[1]
        crc1 = modbus.calc_crc16(req, 0, len(req) - 2)
        crc2 = ustruct.unpack_from('<H', req, len(req) - 2)[0]
        if crc1 != crc2:
            return (status, None)
        if func == 0x03 or func == 0x04:
            resfunc = func
            addr = ustruct.unpack_from('!H', req, 2)[0]
            cnt = ustruct.unpack_from('!H', req, 4)[0]
            if addr > self.__start_address or (addr - self.__start_address + cnt) * 2 > self.__buf_size:
                error = 0x02
            if error == 0:
                data = self.__buf[(addr - self.__start_address) * 2:(addr - self.__start_address + cnt) * 2 + 1]
                resp = ustruct.pack('!BBB{0:d}sI'.format(len(data)), sub, resfunc, len(data), data, 0x0000)  
        elif func == 0x01:
            pass
        if error > 0:
            status = 1
            resp = ustruct.pack('!BBBI', sub, func + 0x80, error, 0)
        if resp != None:
            res = bytearray(resp)
            del resp
            crc = modbus.calc_crc16(res, 0, len(res) - 2)
            ustruct.pack_into('<H', res, len(res) - 2, crc)
        return (status, res)

    def get_offset(self, addr):
        return (addr - self.__start_address) * 2

    def read_float(self, addr, byteorder = 0):
        offset = self.get_offset(addr)
        fmt = '!f'
        if byteorder == 1:
            fmt = '<f'
        return ustruct.unpack_from(fmt, self.__buf, offset)[0]

    def read_uint16(self, addr, byteorder = 0):
        offset = self.get_offset(addr)
        fmt = '!H'
        if byteorder == 1:
            fmt = '<H'
        return ustruct.unpack_from(fmt, self.__buf, offset)[0]

    def read_uint32(self, addr, byteorder = 0):
        offset = self.get_offset(addr)
        fmt = '!I'
        if byteorder == 1:
            fmt = '<I'
        return ustruct.unpack_from(fmt, self.__buf, offset)[0]
    

    def write_bytes(self, addr, data):
        offset = self.get_offset(addr)
        ustruct.pack_into('{0}s'.format(len(data)), self.__buf, offset, data)

    def write_float(self, addr, value):
        offset = self.get_offset(addr)
        ustruct.pack_into('!f', self.__buf, offset, value)

    def write_uint16(self, addr, value):
        offset = self.get_offset(addr)
        ustruct.pack_into('!H', self.__buf, offset, value)
    
    def write_uint32(self, addr, value):
        offset = self.get_offset(addr)
        ustruct.pack_into('!I', self.__buf, offset, value)



    
            




    